/*
 * Motor_Control_Fcns.c
 *
 *  Created on: Mar 25, 2024
 *      Author: Yiwen Zhu
 */

#include "Motor_Control_Fcns.h"


// ########################  PRE Calculation Functions ###################

// Judge phase over current event
#define OCP_MAX 4000
#define OCP_MIN 100
void overcur_protection(uint16_t* buf){
	if(buf[0]>OCP_MAX||buf[1]>OCP_MAX||buf[0]<OCP_MIN||buf[1]<OCP_MIN){
		STATE_ctl_commond(STOP);
	}
}

// Get the real values of phase current (A).
void get_current_sample_real_value(void){
	clarke1.As = ((float)(AD_BUF[0] - DEF_OFFSET_AD_1))*DEF_AD_TO_REAL_CURRENT_A;
	clarke1.Bs = ((float)(AD_BUF[1] - DEF_OFFSET_AD_2))*DEF_AD_TO_REAL_CURRENT_B;
	clarke1.Cs = -(clarke1.As + clarke1.Bs);

	simp_svm1.Udc = AD_BUF[2]*DEF_AD_TO_REAL_VDC;
}

// Sample and calculate the real electrical angle, form 0 to 2Pi
float get_electrical_angle_real_value(void){
	float theta_temp;
	static int AD[2]={200,3000};
	int raw_ang = AD_BUF[3];
	if(raw_ang<AD[0]) AD[0]=raw_ang;
	else if(raw_ang>AD[1])	AD[1]=raw_ang;

	theta_temp = (raw_ang-26)*DEF_ANG_TO_DUAL_PI-DEF_ANG_OFFEST_DUAL_PI;

	return theta_temp;
}

// Calculate real 'electrical' angular speed form the absolute position
// 0-4095 => 0~2pi*(pole pairs)(rad)
// For a 7Np motor, 1r/s = 44rad/s
float get_electrical_angular_speed_real_value(float angle,int periods,float lpf_alpha){
	static float Ang_Last = 0.0f; // record 3 period angle
	static float Ang_Change_Last = 0;
	static float We = 0.0f;
	static char  Prd_Cnt = 0.0f;
	static float Change_Sum = 0.0f;
	static float Lpf_Last = 0.2f;

	float angle_change = Ang_Last - angle;	// the angle changed in one period

	// cross-zero check
	if((angle_change >= 0.5f) )			angle_change = Ang_Change_Last;
	else if ((angle_change <= -0.5f))	angle_change = -Ang_Change_Last;

	// Calculate  electrical angular speed (rad/s), by using the electrical angle
	if(++Prd_Cnt <= periods)	Change_Sum += angle_change;
	else{
		We = Change_Sum/(DRV_TS*periods);
		Prd_Cnt = 0;
		Change_Sum = 0;
	}

	Ang_Last = angle;
	Ang_Change_Last = angle_change;

	We = lpf_alpha*We +(1-lpf_alpha)*Lpf_Last;
	Lpf_Last = We;
	return We;	// In rad/s electrical speed
}

// Update the output of SVPWM to the hrtim registers
void svm_tim_update(float Ta,float Tb,float Tc){
	u16 temp[3];

//	Right aligned
	if(DEF_CTL_STATE == RUN){ // Reference wave >= Carry wave, out put 1

//		temp[0] = (u16)( (DEF_SVM_TO_CMP-(Ta+1)*(DEF_SVM_TO_CMP>>1)) )>>1; // DEF_SVM_TO_CMP
//		temp[1] = (u16)( (DEF_SVM_TO_CMP-(Tb+1)*(DEF_SVM_TO_CMP>>1)) )>>1;
//		temp[2] = (u16)( (DEF_SVM_TO_CMP-(Tc+1)*(DEF_SVM_TO_CMP>>1)) )>>1;
//
//		__HAL_HRTIM_SETCOMPARE(&hhrtim1, HRTIM_TIMERINDEX_TIMER_A, HRTIM_COMPAREUNIT_1, (temp[0]+5)); // Rising edge compare event
//		__HAL_HRTIM_SETCOMPARE(&hhrtim1, HRTIM_TIMERINDEX_TIMER_A, HRTIM_COMPAREUNIT_2, (DEF_SVM_TO_CMP-temp[0]-5)); // Falling edge compare event
//
//		__HAL_HRTIM_SETCOMPARE(&hhrtim1, HRTIM_TIMERINDEX_TIMER_B, HRTIM_COMPAREUNIT_1, (temp[1]+5));// Rising edge compare event
//		__HAL_HRTIM_SETCOMPARE(&hhrtim1, HRTIM_TIMERINDEX_TIMER_B, HRTIM_COMPAREUNIT_2, (DEF_SVM_TO_CMP-temp[1]-5));// Falling edge compare event
//
//		__HAL_HRTIM_SETCOMPARE(&hhrtim1, HRTIM_TIMERINDEX_TIMER_C, HRTIM_COMPAREUNIT_1, (temp[2]+5));// Rising edge compare event
//		__HAL_HRTIM_SETCOMPARE(&hhrtim1, HRTIM_TIMERINDEX_TIMER_C, HRTIM_COMPAREUNIT_2, (DEF_SVM_TO_CMP-temp[2]-5));// Falling edge compare event

		//	Simple SVM
//		temp[0] = (u16)( (DEF_SVM_TO_CMP-(Ta+1)*(DEF_SVM_TO_CMP>>1)) ); // DEF_SVM_TO_CMP
//		temp[1] = (u16)( (DEF_SVM_TO_CMP-(Tb+1)*(DEF_SVM_TO_CMP>>1)) );
//		temp[2] = (u16)( (DEF_SVM_TO_CMP-(Tc+1)*(DEF_SVM_TO_CMP>>1)) );

//		__HAL_HRTIM_SETCOMPARE(&hhrtim1, HRTIM_TIMERINDEX_TIMER_A, HRTIM_COMPAREUNIT_1, (temp[0]+5)); // Rising edge compare event
//		__HAL_HRTIM_SETCOMPARE(&hhrtim1, HRTIM_TIMERINDEX_TIMER_A, HRTIM_COMPAREUNIT_2, (DEF_SVM_TO_CMP-temp[0]-5)); // Falling edge compare event
//
//		__HAL_HRTIM_SETCOMPARE(&hhrtim1, HRTIM_TIMERINDEX_TIMER_B, HRTIM_COMPAREUNIT_1, (temp[1]+5));// Rising edge compare event
//		__HAL_HRTIM_SETCOMPARE(&hhrtim1, HRTIM_TIMERINDEX_TIMER_B, HRTIM_COMPAREUNIT_2, (DEF_SVM_TO_CMP-temp[1]-5));// Falling edge compare event
//
//		__HAL_HRTIM_SETCOMPARE(&hhrtim1, HRTIM_TIMERINDEX_TIMER_C, HRTIM_COMPAREUNIT_1, (temp[2]+5));// Rising edge compare event
//		__HAL_HRTIM_SETCOMPARE(&hhrtim1, HRTIM_TIMERINDEX_TIMER_C, HRTIM_COMPAREUNIT_2, (DEF_SVM_TO_CMP-temp[2]-5));// Falling edge compare event

		temp[0] = (u16)( ((Ta)*(DEF_SVM_TO_CMP)) -5); // DEF_SVM_TO_CMP
		temp[1] = (u16)( ((Tb)*(DEF_SVM_TO_CMP)) -5);
		temp[2] = (u16)( ((Tc)*(DEF_SVM_TO_CMP)) -5);
		__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, temp[0]);    //修改比较值，修改占空比
		__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_2, temp[1]);    //修改比较值，修改占空比
		__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, temp[2]);    //修改比较值，修改占空比

	}
}

// ######################## Control Functions #####################

// Open-loop control function
void CTL_openloop_foc_task(void){
	char toggle_flag = 0;
	float theta,we;

	theta = GetAngle_Without_Track_DMA((uint16_t)M_NP)-DEF_ANG_OFFEST_DUAL_PI;
	we = get_electrical_angular_speed_real_value(theta,20,0.2f);
	G_Theta = theta;
	G_We = we;


	if(pi_iq1.Ref>0) //pi_iq1.Ref<0
		theta += pi_iq1.Ref/100;				//	自增角度
	else
		theta -= pi_iq1.Ref/100;				//	自减角度
	if(theta >= DUAL_PI){			// _IQ(1.0)
		theta = 0.0f;
	}

	get_current_sample_real_value();
	CLARKE_MACRO_CP(clarke1);

	// Park transformation
	park1.Alpha = clarke1.Alpha;
	park1.Beta = clarke1.Beta;
	park1.sin = SIN_MACRO(theta);	// More efficiency !
	park1.cos = COS_MACRO(theta);
	PARK_MACRO(park1);

	simp_svm1.Ud = 0.0f;
	simp_svm1.Uq = 2.0f;
	simp_svm1.Sin = park1.sin;
	simp_svm1.Cos = park1.cos;
	simplified_svpwm_calc();

	svm_tim_update(simp_svm1.Ta,simp_svm1.Tb,simp_svm1.Tc);

//	if(toggle_flag==0)
//		svm_tim_update(0.0,0.5,1.0);
//	else
//		svm_tim_update(1.0,0.5,1.0);

	toggle_flag = ~toggle_flag;
}

// Absolute angle sensor initial offset test
void CTL_init_ang(void){
	G_Theta = GetAngle_Without_Track_DMA((uint16_t)M_NP)-DEF_ANG_OFFEST_DUAL_PI;

//	svm1.Ualpha =  0.5f;// ipark1.alpha;//
//	svm1.Ubeta =   0.0f;// 	ipark1.beta;//
//
//	SVGENDQ_MACRO(svm1);
//
//	svm_tim_update(svm1.Ta,svm1.Tb,svm1.Tc);
	// SVM calculation
	simp_svm1.Ud = 2.0f;
	simp_svm1.Uq = 0;
	simp_svm1.Sin = 0;
	simp_svm1.Cos = 1.0f;
	simp_svm1.KUdc = 0.66667f/simp_svm1.Udc;
	simplified_svpwm_calc(); // the simple method to calculate SVPWM
	// Update the hrtim compare value for generate PWM
	svm_tim_update(simp_svm1.Ta,simp_svm1.Tb,simp_svm1.Tc);
}


// Current-loop control function
void CTL_current_loop_foc_task(void){
	float theta,we;
	static int Spd_Ctl_Tick = 0;

	// First, read raw ADC value and convert them to real current value.
	get_current_sample_real_value();
	// Use Clarke transform get current in alpha-beta axis.
	// Note the Clarke is constant-power transformation, thus the torque no need the (3/2)
	CLARKE_MACRO_CA(clarke1);

	// Calculate speed and position
//	theta = get_electrical_angle_real_value();
	theta = GetAngle_Without_Track_DMA((uint16_t)M_NP)-DEF_ANG_OFFEST_DUAL_PI;
	we = get_electrical_angular_speed_real_value(theta,50,0.1f);
	while(theta>DUAL_PI)	theta -= DUAL_PI;
	while(theta<0)	theta += DUAL_PI;
	G_Theta = theta;
	G_We = we;

//	static int Hfi_Init_Tick = 0;
//	if(++Hfi_Init_Tick>30000){
//		if(theta-hfi1.Theta>0.5)	hfi1.Theta-=PI;
//		else if(theta-hfi1.Theta<-0.5)	hfi1.Theta-=PI;
		theta = hfi1.Theta;
//		Hfi_Init_Tick = 30005;
//	}

	// FHI injection
	// Enable the sensorless
	hfi1.Ial = clarke1.Alpha;
	hfi1.Ibe = clarke1.Beta;
	HFI_calc_task();		// Must calculate the angle first, because we need the sign(Uinj)!
	while(hfi1.Theta>DUAL_PI)	hfi1.Theta -= DUAL_PI;
	while(hfi1.Theta<0)	hfi1.Theta += DUAL_PI;


	if(++Spd_Ctl_Tick>5){
		pi_spd1.Fdb = -we/628.0f;
		PID_ST_MACRO(pi_spd1);
		Spd_Ctl_Tick = 0;
	}

	// Park transformation
	park1.Alpha = clarke1.Alpha;
	park1.Beta = clarke1.Beta;
	park1.sin = SIN_MACRO(theta);	// More efficiency !
	park1.cos = COS_MACRO(theta);
	PARK_MACRO(park1);

	// Current-loop PID regulator
//	pi_id1.Ref = 0.0f;
	pi_id1.Fdb = park1.Ds;
	PI_ST_MACRO(pi_id1);

//	float  lpf_al1 = 0.1;
//	pi_spd1.Out = -pi_spd1.Out;
//	pi_iq1.Ref = (pi_spd1.Out*lpf_al1)+(1-lpf_al1)*pi_iq1.Ref;
	pi_iq1.Fdb = park1.Qs;
	PI_ST_MACRO(pi_iq1);


	// SVM calculation
	simp_svm1.Ud = pi_id1.Out+hfi1.Uout;
	simp_svm1.Uq = pi_iq1.Out;
	simp_svm1.Sin = park1.sin;
	simp_svm1.Cos = park1.cos;
	simp_svm1.KUdc = 0.66667f/simp_svm1.Udc;
	simplified_svpwm_calc(); // the simple method to calculate SVPWM
	// Update the hrtim compare value for generate PWM
	svm_tim_update(simp_svm1.Ta,simp_svm1.Tb,simp_svm1.Tc);	//DEF_SVM_TO_CMP
}

void CTL_current_loop_MPCC_task(void){
	float theta,we;
	static int Spd_Ctl_Tick = 0;

	// First, read raw ADC value and convert them to real current value.
	get_current_sample_real_value();
	// Use Clarke transform get current in alpha-beta axis.
	// Note the Clarke is constant-amplitude transformation, thus the torque need the (3/2)
	CLARKE_MACRO_CP(clarke1);

	// Calculate speed and position
	//theta = get_electrical_angle_real_value();
	theta = GetAngle_Without_Track_DMA((uint16_t)M_NP) - DEF_ANG_OFFEST_DUAL_PI;
	we = get_electrical_angular_speed_real_value(theta,50,0.1f);
	G_Theta = theta;
	G_We = we;

	// Speed loop PI controller
	//For a 7Np motor, 1r/s = 44rad/s, 1PU = 100r/s
//	pi_spd1.Ref = 0.15f;
	if(++Spd_Ctl_Tick>5){
		pi_spd1.Fdb = -we/628.0f;
		PID_ST_MACRO(pi_spd1);
		Spd_Ctl_Tick = 0;
	}


	// Get the currents in d-q axis
	park1.Alpha = clarke1.Alpha;
	park1.Beta = clarke1.Beta;
	park1.sin = SIN_MACRO((float)theta);	// More efficiency !
	park1.cos = COS_MACRO((float)theta);
	PARK_MACRO(park1);

	// MPCC controller
	mpcc1.Ref_Id = pi_id1.Ref;
	mpcc1.Ref_Iq = pi_iq1.Ref;
//	mpcc1.Ref_Iq = pi_spd1.Out;
	mpcc1.Fdb_Id = park1.Ds;
	mpcc1.Fdb_Iq = park1.Qs;
	mpcc1.We = we;
	mpcc1.sin = park1.sin;
	mpcc1.cos = park1.cos;
	mpcc_task();

	__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, (mpcc1.Ta));    //修改比较值，修改占空比
	__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_2, (mpcc1.Tb));    //修改比较值，修改占空比
	__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, (mpcc1.Tc));    //修改比较值，修改占空比

}

void CTL_current_loop_MPDTC_task(void){
	float theta,we;
	static int Spd_Ctl_Tick = 0;

	// First, read raw ADC value and convert them to real current value.
	get_current_sample_real_value();
	// Use Clarke transform get current in alpha-beta axis.
	// Note if the Clarke is constant-amplitude transformation, thus the torque need the (3/2)
	CLARKE_MACRO_CP(clarke1);

	// Calculate speed and position
	//theta = get_electrical_angle_real_value();
	theta = GetAngle_Without_Track_DMA((uint16_t)M_NP) - DEF_ANG_OFFEST_DUAL_PI;
	we = get_electrical_angular_speed_real_value(theta,50,0.1f);
	G_Theta = theta;
	G_We = we;

	// Speed loop PI controller
	//For a 7Np motor, 1r/s = 44rad/s, 1PU = 100r/s
//	pi_spd1.Ref = 0.15f;
	if(++Spd_Ctl_Tick>5){
		pi_spd1.Fdb = -we/628.0f;
		PID_ST_MACRO(pi_spd1);
		Spd_Ctl_Tick = 0;
	}


	// Get the currents in d-q axis
	park1.Alpha = clarke1.Alpha;
	park1.Beta = clarke1.Beta;
	park1.sin = SIN_MACRO((float)theta);	// More efficiency !
	park1.cos = COS_MACRO((float)theta);
	PARK_MACRO(park1);

	// MPDTC controller
	mpdtc1.Ref_Flux = (1+pi_id1.Ref)*M_PSIr;	// PM flux 0.0045
	mpdtc1.Ref_Trq = pi_iq1.Ref*M_TRQ_RATED;		// 0.2NM
//	mpdtc1.Ref_Iq = pi_spd1.Out;
	mpdtc1.Fdb_Id = park1.Ds;
	mpdtc1.Fdb_Iq = park1.Qs;
	mpdtc1.We = we;
	mpdtc1.sin = park1.sin;
	mpdtc1.cos = park1.cos;
	mpdtc_task();	// Could run at 30kHz (without parameter estimation)
//	mpdtc_evenhand_task();	// Consume too much time, just run at 15kHz

//	est1.Vdc = simp_svm1.Udc;
//	est1.Theta = theta;
//	INDUCTANCE_EST_FUNC(mpdtc1.Index_Last2);

	__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, (mpdtc1.Ta));    //修改比较值，修改占空比
	__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_2, (mpdtc1.Tb));    //修改比较值，修改占空比
	__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, (mpdtc1.Tc));    //修改比较值，修改占空比

}


